package com;

import cn.hutool.core.util.RandomUtil;
import cn.hutool.json.JSONUtil;
import com.google.common.collect.Lists;
import com.model.Node;
import com.model.TargetNode;
import com.util.PositionUtil;
import org.ejml.simple.SimpleMatrix;

import java.util.Arrays;
import java.util.List;

/**
 * <p>
 * 三点二维定位
 * </p>
 *
 * @author : zb-z
 * @version : 1.0
 * @date : 2023/10/16 09:15
 */
public class ThreeDPosition {


    public static void main (String[] args) {
        // 初始化场地
        int limit = 100;

        // 观测站节点初始化
        int nodeNumber = 3;
        int avgNumber = 5;
        List<Node> nodeList = Lists.newArrayList();
        List<List<Double>> xyzList = Lists.newArrayList();
        for (int i = 0; i < nodeNumber; i++) {
            Node tmpNode = new Node();
            tmpNode.setX(limit * RandomUtil.randomDouble(0, 1));
            tmpNode.setY(limit * RandomUtil.randomDouble(0, 1));
            tmpNode.setZ(0);
            tmpNode.setD(Math.pow(tmpNode.getX(), 2) + Math.pow(tmpNode.getY(), 2) + Math.pow(tmpNode.getZ(), 2));
            nodeList.add(tmpNode);

            List<Double> xyz  = Lists.newArrayList();
            xyz.add(tmpNode.getX());
            xyz.add(tmpNode.getY());
            xyz.add(tmpNode.getZ());
            xyzList.add(xyz);
        }

        System.out.println("基站坐标：");
        System.out.println(JSONUtil.toJsonStr(xyzList));

        // 被观测节点初始化
        TargetNode targetNode = new TargetNode();
        targetNode.setX(limit * RandomUtil.randomDouble(0, 1));
        targetNode.setY(limit * RandomUtil.randomDouble(0, 1));
        targetNode.setZ(0);

        //模拟各观测站对目标探测20次Rssi值
        double[][] Rssi = new double[nodeList.size()][avgNumber];
        for (int i = 0; i < nodeList.size(); i++) {
            for (int j = 0; j < avgNumber; j++) {
                final double distance = PositionUtil.getDistance(nodeList.get(i), targetNode);
                Rssi[i][j] = PositionUtil.getRssiByDistance(distance);
            }
        }

        // 储存20次观测的平均值
        double[] rssiAvg = new double[nodeList.size()];
        for (int i = 0; i < nodeList.size(); i++) {
            rssiAvg[i]= Arrays.stream(Rssi[i]).sequential().sum()/avgNumber;
        }

        // 根据Rssi求观测距离
        double[] distances = new double[nodeList.size()];
        for (int i = 0; i < nodeList.size(); i++) {
            distances[i] = PositionUtil.getDistanceByRssi(rssiAvg[i]);
        }

        // 根据观测距离用最小二乘法估计目标位置
        final SimpleMatrix A = new SimpleMatrix(nodeList.size() - 1, 2);
        final SimpleMatrix B = new SimpleMatrix(nodeList.size() - 1, 1);
        A.fill(0.0);
        B.fill(0.0);
        for (int i = 0; i < nodeList.size() - 1; i++) {
            // A 矩阵 和 B 矩阵
            A.setRow(i, 0, 2 * (nodeList.get(i + 1).getX() - nodeList.get(0).getX()), 2 * (nodeList.get(i + 1).getY() - nodeList.get(0).getY()));
            B.set(i, Math.pow(distances[0],2)-Math.pow(distances[i + 1],2)+ nodeList.get(i + 1).getD() - nodeList.get(0).getD());
        }
//        A.print();
//        B.print();
        SimpleMatrix AT = A.transpose();
        SimpleMatrix ATA = AT.mult(A);
        SimpleMatrix ATb = AT.mult(B);

        SimpleMatrix Estimate = ATA.invert().mult(ATb);

        TargetNode estimateNode = new TargetNode();
        estimateNode.setX(Estimate.get(0, 0));
        estimateNode.setY(Estimate.get(1, 0));


        List<Double> xyz  = Lists.newArrayList();
        System.out.println("目标坐标：");
        xyz.clear();
        xyz.add(targetNode.getX());
        xyz.add(targetNode.getY());
        System.out.println(JSONUtil.toJsonStr(xyz));

        System.out.println("预计坐标：");
        xyz.clear();
        xyz.add(estimateNode.getX());
        xyz.add(estimateNode.getY());
        System.out.println(JSONUtil.toJsonStr(xyz));

        System.out.println("计算误差：");
        System.out.println(PositionUtil.getDistance(targetNode, estimateNode));

    }

}

